#include "../platform.h"
#include <rtl/km/cpu.h>

#define MSDC_FIFOCS (0x14)
#define MSDC_TXDATA (0x18)
#define MSDC_RXDATA (0x1c)
#define MSDC_CMD (0x34)
#define MSDC_ARG (0x38)
#define MSDC_STS (0x3c)
#define MSDC_FIFOCS_RXCNT 0x000000FF
#define MSDC_FIFOCS_TXCNT 0x00FF0000
#define MSDC_FIFO_SIZE 128
#define MSDC_STS_SDCBUSY 1

#define MMC_SEND_EXT_CSD 8
#define MMC_READ_SINGLE_BLOCK 17
#define MMC_WRITE_BLOCK 24

#define MMC_CONTROLLER_INITIALIZED (1)

typedef struct _MMC_CONTROLLER
{
    UINTPTR Base;
    UINT32 Flags;
} MMC_CONTROLLER, *PMMC_CONTROLLER;

MMC_CONTROLLER AgpMmcControllers[] = {{.Base = MSDC0_BASE, .Flags = 0}};

#define MMC_WRITE32(controller, offset, data) WRITE32((controller)->Base + (offset), (data))
#define MMC_READ32(controller, offset) READ32((controller)->Base + (offset))

N1_STATUS MtkMmcInit(INT ControllerIndex)
{
    if (ControllerIndex >= ARRAY_SIZE(AgpMmcControllers))
        return N1_OBJECT_NOT_FOUND;

    PMMC_CONTROLLER Controler = &AgpMmcControllers[ControllerIndex];
    Controler->Flags |= MMC_CONTROLLER_INITIALIZED;

    return 0;
}

N1_STATUS MtkMmcCommand(INT ControllerIndex, UINT8 Opcode, UINT32 Argument)
{
    if (ControllerIndex >= ARRAY_SIZE(AgpMmcControllers))
        return N1_OBJECT_NOT_FOUND;

    PMMC_CONTROLLER Controller = &AgpMmcControllers[ControllerIndex];
    if (!(Controller->Flags & MMC_CONTROLLER_INITIALIZED))
        return N1_INVALID_STATE;

    UINT32 Command;

    Command = 0x2000880 | Opcode;
    if (Opcode == MMC_WRITE_BLOCK)
        Command |= 0x2000;

    while (MMC_READ32(Controller, MSDC_STS) & MSDC_STS_SDCBUSY)
    {
        RtlCpuRelax();
    }

    MMC_WRITE32(Controller, MSDC_ARG, Argument);
    MMC_WRITE32(Controller, MSDC_CMD, Command);

    return 0;
}

N1_STATUS MtkpMmcRead(PMMC_CONTROLLER Controller, PVOID _Buffer, SIZE Count)
{
    UINT32 *Buffer = (UINT32 *)_Buffer;
    while (Count > 0)
    {
        if ((MMC_READ32(Controller, MSDC_FIFOCS) & MSDC_FIFOCS_RXCNT) >= MSDC_FIFO_SIZE)
        {
            for (INT I = 0; I < MSDC_FIFO_SIZE / 4; I++)
            {
                *Buffer = MMC_READ32(Controller, MSDC_RXDATA);
                Buffer++;
            }
            Count -= MSDC_FIFO_SIZE;
        }
        else
            RtlCpuRelax();
    }
    return 0;
}

N1_STATUS MtkpMmcWrite(PMMC_CONTROLLER Controller, PVOID _Buffer, SIZE Count)
{
    UINT32 *Buffer = (UINT32 *)_Buffer;
    while (Count > 0)
    {
        if ((MMC_READ32(Controller, MSDC_FIFOCS) & MSDC_FIFOCS_TXCNT) == 0)
        {
            for (INT I = 0; I < MSDC_FIFO_SIZE / 4; I++)
            {
                MMC_WRITE32(Controller, MSDC_TXDATA, *Buffer);
                Buffer++;
            }
            Count -= MSDC_FIFO_SIZE;
        }
        else
            RtlCpuRelax();
    }

    return 0;
}

N1_STATUS MtkMmcRead(INT ControllerIndex, PVOID Buffer, SIZE NumBlocks, UINT32 BlockOffset)
{
    N1_STATUS Status;
    if (ControllerIndex >= ARRAY_SIZE(AgpMmcControllers))
        return N1_OBJECT_NOT_FOUND;

    PMMC_CONTROLLER Controller = &AgpMmcControllers[ControllerIndex];
    if (!(Controller->Flags & MMC_CONTROLLER_INITIALIZED))
        return N1_INVALID_STATE;

    while (NumBlocks > 0)
    {
        Status = MtkMmcCommand(ControllerIndex, MMC_READ_SINGLE_BLOCK, BlockOffset);
        if (!N1_SUCCESS(Status))
            return Status;

        MtkpMmcRead(Controller, Buffer, 512);

        Buffer = (PVOID)((UINTPTR)Buffer + 512);
        BlockOffset++;
        NumBlocks--;
    }

    return 0;
}

N1_STATUS MtkMmcWrite(INT ControllerIndex, PVOID Buffer, SIZE NumBlocks, UINT32 BlockOffset)
{
    N1_STATUS Status;
    if (ControllerIndex >= ARRAY_SIZE(AgpMmcControllers))
        return N1_OBJECT_NOT_FOUND;

    PMMC_CONTROLLER Controller = &AgpMmcControllers[ControllerIndex];
    if (!(Controller->Flags & MMC_CONTROLLER_INITIALIZED))
        return N1_INVALID_STATE;

    while (NumBlocks > 0)
    {
        Status = MtkMmcCommand(ControllerIndex, MMC_WRITE_BLOCK, BlockOffset);
        if (!N1_SUCCESS(Status))
            return Status;

        MtkpMmcWrite(Controller, Buffer, 512);

        Buffer = (PVOID)((UINTPTR)Buffer + 512);
        BlockOffset++;
        NumBlocks--;
    }

    return 0;
}

UINT32 MtkGetMmcControllerCount()
{
    return ARRAY_SIZE(AgpMmcControllers);
}